#include "FreeRTOS.h"
#include "task.h"

#include "nxbot.h"
#include "sonar.h"

#define SONAR_MAX_IDLE_TIME 20

static void sonarTask(void *pvParameters);


void executeSonarTask(globals_t *pvParameters)
{

	xTaskCreate(sonarTask,(const signed portCHAR * const)"sonarTask",sonarTask_stackSize,(void *)pvParameters,tskIDLE_PRIORITY + 2,( xTaskHandle * ) NULL);
}

//tarea que solo actualiza la info de los sensores IR
//TODO: polling de bumpers, y de bateria????
static void sonarTask(void *pvParameters)
{	

	static unsigned char rxtx = 0;
	static unsigned int val;
	static unsigned char countsIdle = 0;
	unsigned char state = 0;
	static unsigned char actSonar = 0;
	static char newState = -1;
	
	//convertion of parameters 
	globals_t *globals;
	globals=(globals_t *)pvParameters;
	
	//delay
	unsigned int freq;
	portTickType actualTick;
	
	
	for(;;)
	{
		//obtain the frequency of the IRSensors update
		//taskENTER_CRITICAL();
		freq = globals->times.intervalSonars;
		//taskEXIT_CRITICAL();

		if(actSonar >= NXBOT_SONAR_COUNT)
			actSonar = 0;
		
		if(rxtx == 0)
		{
			sensors_setSonar(actSonar,SONAR_CM);//sonar_write(actSonar);
			rxtx = 1;
			
			//time delay according to the sonarInterval value
			actualTick = xTaskGetTickCount();			
			vTaskDelayUntil( &actualTick,(portTickType)freq);	
			
		}
		else
		{
			state = sensors_getSonar(actSonar,&val, newState);// sonar_read(actSonar,&val);
			countsIdle++;
			newState = -1;
	
			if(state == SONAR_READ_READY)
			{
				//convert INCHES to CM
				//val = (float)val*2.54f;
				
				globals->sensors.sonars[actSonar] = val;
				rxtx = 0;
				actSonar++;
				sonar_cleanBuffer();
			}
			else if (countsIdle >= SONAR_MAX_IDLE_TIME)
			{
				countsIdle = 0;
				rxtx = 0;
				newState = -1;
				sonar_cleanBuffer();
			}
		}
		


	}
}



